"""
配置MCP服务器实现
获取配置信息、设置配置参数
"""

from typing import Dict, Any, Optional, List
from mcp.server.fastmcp import FastMCP
from config.robot_config import (
    SERVO_CONFIG,
    KINEMATIC_CONFIG,
    BALANCE_CONFIG,
    COMMUNICATION_CONFIG,
)
from config.sensor_config import (
    VOICE_SENSOR_CONFIG,
    IMU_SENSOR_CONFIG,
    ULTRASONIC_SENSOR_CONFIG,
    CAMERA_SENSOR_CONFIG,
)
from robot_core.config.servo_config import ServoConfig
import os
import yaml


class ConfigServer(FastMCP):
    """配置服务器类"""

    def __init__(self):
        """初始化配置服务器"""
        super().__init__("config_server")

        # 初始化配置管理器
        self.servo_config = ServoConfig()

        # 注册工具
        self._register_tools()

    def _register_tools(self):
        """注册所有配置管理工具"""

        @self.tool()
        async def get_robot_config() -> Dict[str, Any]:
            """获取机器人配置

            Returns:
                Dict[str, Any]: 机器人配置信息
            """
            try:
                return {
                    "status": "success",
                    "data": {
                        "servo": SERVO_CONFIG,
                        "kinematic": KINEMATIC_CONFIG,
                        "balance": BALANCE_CONFIG,
                        "communication": COMMUNICATION_CONFIG,
                    },
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def get_sensor_config() -> Dict[str, Any]:
            """获取传感器配置

            Returns:
                Dict[str, Any]: 传感器配置信息
            """
            try:
                return {
                    "status": "success",
                    "data": {
                        "voice": VOICE_SENSOR_CONFIG,
                        "imu": IMU_SENSOR_CONFIG,
                        "ultrasonic": ULTRASONIC_SENSOR_CONFIG,
                        "camera": CAMERA_SENSOR_CONFIG,
                    },
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def get_servo_config() -> Dict[str, Any]:
            """获取舵机配置

            Returns:
                Dict[str, Any]: 舵机配置信息
            """
            try:
                return {
                    "status": "success",
                    "data": {
                        "control_type": self.servo_config.get_control_type(),
                        "serial_config": self.servo_config.get_serial_config(),
                        "pwm_config": self.servo_config.get_pwm_config(),
                        "servo_config": self.servo_config.get_servo_config(),
                        "mapping": self.servo_config.get_mapping(),
                    },
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def set_servo_id(old_id: int, new_id: int) -> Dict[str, Any]:
            """设置舵机ID

            Args:
                old_id: 原ID
                new_id: 新ID

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                self.servo_config.set_id(old_id, new_id)
                return {
                    "status": "success",
                    "message": f"Set servo ID from {old_id} to {new_id}",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def set_servo_deviation(servo_id: int, deviation: int) -> Dict[str, Any]:
            """设置舵机偏差

            Args:
                servo_id: 舵机ID
                deviation: 偏差值

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                self.servo_config.set_deviation(servo_id, deviation)
                return {
                    "status": "success",
                    "message": f"Set servo {servo_id} deviation to {deviation}",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def set_servo_angle_limit(
            servo_id: int, low: int, high: int
        ) -> Dict[str, Any]:
            """设置舵机角度限制

            Args:
                servo_id: 舵机ID
                low: 最小角度
                high: 最大角度

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                self.servo_config.set_angle_limit(servo_id, low, high)
                return {
                    "status": "success",
                    "message": f"Set servo {servo_id} angle limit to [{low}, {high}]",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def get_servo_state(servo_id: Optional[int] = None) -> Dict[str, Any]:
            """获取舵机状态

            Args:
                servo_id: 舵机ID，如果为None则获取所有舵机状态

            Returns:
                Dict[str, Any]: 舵机状态信息
            """
            try:
                if servo_id is not None:
                    position = self.servo_config.read_position(servo_id)
                    temperature = self.servo_config.read_temperature(servo_id)
                    voltage = self.servo_config.read_voltage(servo_id)
                    deviation = self.servo_config.read_deviation(servo_id)
                    angle_limit = self.servo_config.read_angle_limit(servo_id)

                    return {
                        "status": "success",
                        "data": {
                            "servo_id": servo_id,
                            "position": position,
                            "temperature": temperature,
                            "voltage": voltage,
                            "deviation": deviation,
                            "angle_limit": angle_limit,
                        },
                    }
                else:
                    return {
                        "status": "success",
                        "data": self.servo_config.show_servo_state(),
                    }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def save_config(
            config_type: str, config_data: Dict[str, Any]
        ) -> Dict[str, Any]:
            """保存配置

            Args:
                config_type: 配置类型(robot/sensor/servo)
                config_data: 配置数据

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                config_dir = os.path.join(
                    os.path.dirname(os.path.dirname(__file__)), "config"
                )
                if config_type == "robot":
                    config_path = os.path.join(config_dir, "robot_config.py")
                    with open(config_path, "w", encoding="utf-8") as f:
                        f.write('"""\n机器人配置文件\n"""\n\n')
                        for key, value in config_data.items():
                            f.write(f"{key} = {value}\n")
                elif config_type == "sensor":
                    config_path = os.path.join(config_dir, "sensor_config.py")
                    with open(config_path, "w", encoding="utf-8") as f:
                        f.write('"""\n传感器配置文件\n"""\n\n')
                        for key, value in config_data.items():
                            f.write(f"{key} = {value}\n")
                elif config_type == "servo":
                    config_path = os.path.join(config_dir, "servo.yaml")
                    with open(config_path, "w", encoding="utf-8") as f:
                        yaml.dump(config_data, f, allow_unicode=True)
                else:
                    return {
                        "status": "error",
                        "message": f"Invalid config type: {config_type}",
                    }

                return {"status": "success", "message": f"Saved {config_type} config"}
            except Exception as e:
                return {"status": "error", "message": str(e)}
